
#include "qyOBB.h"

//-----------------------------------------------------------------------
bool OBB::AddPoint(const Vector3 &v)
{

	Matrix3 axis2;
	AABB bounds1, bounds2;

	if ( extents[0] < 0.0f ) {
		extents.Zero();
		center = v;
		axis.Identity();
		return true;
	}

	bounds1[0][0] = bounds1[1][0] = center * axis[0];
	bounds1[0][1] = bounds1[1][1] = center * axis[1];
	bounds1[0][2] = bounds1[1][2] = center * axis[2];
	bounds1[0] -= extents;
	bounds1[1] += extents;
	if ( !bounds1.AddPoint( Vector3( v * axis[0], v * axis[1], v * axis[2] ) ) ) {
		// point is contained in the box
		return false;
	}

	axis2[0] = v - center;
	axis2[0].Normalize();
	axis2[1] = axis[ Min3Index( axis2[0] * axis[0], axis2[0] * axis[1], axis2[0] * axis[2] ) ];
	axis2[1] = axis2[1] - ( axis2[1] * axis2[0] ) * axis2[0];
	axis2[1].Normalize();
	axis2[2].Cross( axis2[0], axis2[1] );

	AxisProjection( axis2, bounds2 );
	bounds2.AddPoint( Vector3( v * axis2[0], v * axis2[1], v * axis2[2] ) );

	// create new box based on the smallest bounds
	if ( bounds1.Volume() < bounds2.Volume() ) {
		center = ( bounds1[0] + bounds1[1] ) * 0.5f;
		extents = bounds1[1] - center;
		center *= axis;
	}
	else {
		center = ( bounds2[0] + bounds2[1] ) * 0.5f;
		extents = bounds2[1] - center;
		center *= axis2;
		axis = axis2;
	}

	return true;
}

//-----------------------------------------------------------------------
bool OBB::IntersectsBox(const OBB &a) const
{
	Vector3 dir;			// vector between centers
	float c[3][3];		// matrix c = axis.Transpose() * a.axis
	float ac[3][3];		// absolute values of c
	float axisdir[3];	// axis[i] * dir
	float d, e0, e1;	// distance between centers and projected extents

	dir = a.center - center;

	// axis C0 + t * A0
	c[0][0] = axis[0] * a.axis[0];
	c[0][1] = axis[0] * a.axis[1];
	c[0][2] = axis[0] * a.axis[2];
	axisdir[0] = axis[0] * dir;
	ac[0][0] = fabsf( c[0][0] );
	ac[0][1] = fabsf( c[0][1] );
	ac[0][2] = fabsf( c[0][2] );

	d = fabsf( axisdir[0] );
	e0 = extents[0];
	e1 = a.extents[0] * ac[0][0] + a.extents[1] * ac[0][1] + a.extents[2] * ac[0][2];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A1
	c[1][0] = axis[1] * a.axis[0];
	c[1][1] = axis[1] * a.axis[1];
	c[1][2] = axis[1] * a.axis[2];
	axisdir[1] = axis[1] * dir;
	ac[1][0] = fabsf( c[1][0] );
	ac[1][1] = fabsf( c[1][1] );
	ac[1][2] = fabsf( c[1][2] );

	d = fabsf( axisdir[1] );
	e0 = extents[1];
	e1 = a.extents[0] * ac[1][0] + a.extents[1] * ac[1][1] + a.extents[2] * ac[1][2];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A2
	c[2][0] = axis[2] * a.axis[0];
	c[2][1] = axis[2] * a.axis[1];
	c[2][2] = axis[2] * a.axis[2];
	axisdir[2] = axis[2] * dir;
	ac[2][0] = fabsf( c[2][0] );
	ac[2][1] = fabsf( c[2][1] );
	ac[2][2] = fabsf( c[2][2] );

	d = fabsf( axisdir[2] );
	e0 = extents[2];
	e1 = a.extents[0] * ac[2][0] + a.extents[1] * ac[2][1] + a.extents[2] * ac[2][2];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * B0
	d = fabsf( a.axis[0] * dir );
	e0 = extents[0] * ac[0][0] + extents[1] * ac[1][0] + extents[2] * ac[2][0];
	e1 = a.extents[0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * B1
	d = fabsf( a.axis[1] * dir );
	e0 = extents[0] * ac[0][1] + extents[1] * ac[1][1] + extents[2] * ac[2][1];
	e1 = a.extents[1];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * B2
	d = fabsf( a.axis[2] * dir );
	e0 = extents[0] * ac[0][2] + extents[1] * ac[1][2] + extents[2] * ac[2][2];
	e1 = a.extents[2];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A0xB0
	d = fabsf( axisdir[2] * c[1][0] - axisdir[1] * c[2][0] );
	e0 = extents[1] * ac[2][0] + extents[2] * ac[1][0];
	e1 = a.extents[1] * ac[0][2] + a.extents[2] * ac[0][1];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A0xB1
	d = fabsf( axisdir[2] * c[1][1] - axisdir[1] * c[2][1] );
	e0 = extents[1] * ac[2][1] + extents[2] * ac[1][1];
	e1 = a.extents[0] * ac[0][2] + a.extents[2] * ac[0][0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A0xB2
	d = fabsf( axisdir[2] * c[1][2] - axisdir[1] * c[2][2] );
	e0 = extents[1] * ac[2][2] + extents[2] * ac[1][2];
	e1 = a.extents[0] * ac[0][1] + a.extents[1] * ac[0][0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A1xB0
	d = fabsf( axisdir[0] * c[2][0] - axisdir[2] * c[0][0] );
	e0 = extents[0] * ac[2][0] + extents[2] * ac[0][0];
	e1 = a.extents[1] * ac[1][2] + a.extents[2] * ac[1][1];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A1xB1
	d = fabsf( axisdir[0] * c[2][1] - axisdir[2] * c[0][1] );
	e0 = extents[0] * ac[2][1] + extents[2] * ac[0][1];
	e1 = a.extents[0] * ac[1][2] + a.extents[2] * ac[1][0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A1xB2
	d = fabsf( axisdir[0] * c[2][2] - axisdir[2] * c[0][2] );
	e0 = extents[0] * ac[2][2] + extents[2] * ac[0][2];
	e1 = a.extents[0] * ac[1][1] + a.extents[1] * ac[1][0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A2xB0
	d = fabsf( axisdir[1] * c[0][0] - axisdir[0] * c[1][0] );
	e0 = extents[0] * ac[1][0] + extents[1] * ac[0][0];
	e1 = a.extents[1] * ac[2][2] + a.extents[2] * ac[2][1];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A2xB1
	d = fabsf( axisdir[1] * c[0][1] - axisdir[0] * c[1][1] );
	e0 = extents[0] * ac[1][1] + extents[1] * ac[0][1];
	e1 = a.extents[0] * ac[2][2] + a.extents[2] * ac[2][0];
	if ( d > e0 + e1 ) {
		return false;
	}

	// axis C0 + t * A2xB2
	d = fabsf( axisdir[1] * c[0][2] - axisdir[0] * c[1][2] );
	e0 = extents[0] * ac[1][2] + extents[1] * ac[0][2];
	e1 = a.extents[0] * ac[2][1] + a.extents[1] * ac[2][0];
	if ( d > e0 + e1 ) {
		return false;
	}
	return true;	
}

//-----------------------------------------------------------------------
bool OBB::LineIntersection(const Vector3 &start, const Vector3 &end) const
{
	float ld[3];
	Vector3 lineDir = 0.5f * ( end - start );
	Vector3 lineCenter = start + lineDir;
	Vector3 dir = lineCenter - center;

	ld[0] = fabsf( lineDir * axis[0] );
	if ( fabsf( dir * axis[0] ) > extents[0] + ld[0] ) {
		return false;
	}

	ld[1] = fabsf( lineDir * axis[1] );
	if ( fabsf( dir * axis[1] ) > extents[1] + ld[1] ) {
		return false;
	}

	ld[2] = fabsf( lineDir * axis[2] );
	if ( fabsf( dir * axis[2] ) > extents[2] + ld[2] ) {
		return false;
	}

	Vector3 cross = lineDir.Cross( dir );

	if ( fabsf( cross * axis[0] ) > extents[1] * ld[2] + extents[2] * ld[1] ) {
		return false;
	}

	if ( fabsf( cross * axis[1] ) > extents[0] * ld[2] + extents[2] * ld[0] ) {
		return false;
	}

	if ( fabsf( cross * axis[2] ) > extents[0] * ld[1] + extents[1] * ld[0] ) {
		return false;
	}

	return true;
}

/*
============
BoxPlaneClip
============
*/
static bool BoxPlaneClip( const float denom, const float numer, float &scale0, float &scale1 ) 
{
	if ( denom > 0.0f ) {
		if ( numer > denom * scale1 ) {
			return false;
		}
		if ( numer > denom * scale0 ) {
			scale0 = numer / denom;
		}
		return true;
	}
	else if ( denom < 0.0f ) {
		if ( numer > denom * scale0 ) {
			return false;
		}
		if ( numer > denom * scale1 ) {
			scale1 = numer / denom;
		}
		return true;
	}
	else {
		return ( numer <= 0.0f );
	}
}

/*
============
Returns true if the ray intersects the box.
The ray can intersect the box in both directions from the start point.
If start is inside the box then scale1 < 0 and scale2 > 0.
============
*/
bool OBB::RayIntersection( const Vector3 &start, const Vector3 &dir, float &scale1, float &scale2 ) 
{
	Vector3 localStart, localDir;

	localStart = ( start - center ) * axis.Transpose();
	localDir = dir * axis.Transpose();

	scale1 = -10000.0f;
	scale2 = 100000.0f;
	return	BoxPlaneClip(  localDir.x, -localStart.x - extents[0], scale1, scale2 ) &&
		BoxPlaneClip( -localDir.x,  localStart.x - extents[0], scale1, scale2 ) &&
		BoxPlaneClip(  localDir.y, -localStart.y - extents[1], scale1, scale2 ) &&
		BoxPlaneClip( -localDir.y,  localStart.y - extents[1], scale1, scale2 ) &&
		BoxPlaneClip(  localDir.z, -localStart.z - extents[2], scale1, scale2 ) &&
		BoxPlaneClip( -localDir.z,  localStart.z - extents[2], scale1, scale2 );
}
